function [CBL, flag] = bodyaxisvertangleazimuth2dcm(vertAngle, azm) %#codegen
%BODYAXISVERTANGLEAZIMUTH2DCM 体坐标系（右前上）xy与水平面的夹角及x轴水平面投影与北向的夹角计算方向余弦矩阵
%
% Tips
% # 参考坐标系L为NED
%
% Input Arguments
% # vertAngle: 2*1向量，体坐标系X、Y轴与水平面的夹角，轴在水平面上方为正，值域[-π/2, π/2]，单位rad
% # azm: 标量，体坐标系X轴在水平面上的投影与北向的夹角（由北向绕顺时针转动到该投影为正），值域[0, 2π)，单位rad
%
% Output Arguments
% # CBL: 3*3*m矩阵

CBL = NaN(3, 3);

% C31 C32
tanTheta = tan(vertAngle(1, 1));
CBL(3, 1) = -sign(tanTheta)*sqrt(tanTheta^2/(1+tanTheta^2));
tanTheta = tan(vertAngle(2, 1));
CBL(3, 2) = -sign(tanTheta)*sqrt(tanTheta^2/(1+tanTheta^2));
% C33 默认Z朝上
CBL(3, 3) = -1*sqrt(1 - CBL(3, 1)^2 - CBL(3, 2)^2);

tanTheta = tan(azm(1, 1));
tmpC11 = sqrt((1-CBL(3, 1)^2)/(1 + tanTheta^2));

for i = 1:2
    % 假设C11为正
    if i == 2
        tmpC11 = -tmpC11;
    end
    CBL(1, 1) = tmpC11;
    CBL(2, 1) = CBL(1, 1) * tanTheta;
    
    % 求C22
    a = 1 + CBL(2, 1)^2/CBL(1, 1)^2;
    b = 2*CBL(3, 1)*CBL(3, 2)*CBL(2, 1)/CBL(1, 1)^2;
    c = (CBL(3, 1)*CBL(3, 2))^2/CBL(1, 1)^2 - 1 + CBL(3, 2)^2;
    tmpC22 = real(roots([a b c]));
    
    % 求C23
    a = 1 + CBL(2, 1)^2/CBL(1, 1)^2;
    b = 2*CBL(3, 1)*CBL(3, 3)*CBL(2, 1)/CBL(1, 1)^2;
    c = (CBL(3, 1)*CBL(3, 3))^2/CBL(1, 1)^2 - 1 + CBL(3, 3)^2;
    tmpC23 = real(roots([a b c]));
    
    % 假设C22并判断
    for j = 1:2
        flag = [ 0 0 0 ];
        CBL(2, 2) = tmpC22(j);
        CBL(1, 2) = (-CBL(2, 1)*CBL(2, 2) - CBL(3, 1)*CBL(3, 2))/CBL(1, 1);
        if abs(CBL(1, 1)*CBL(1, 2) + CBL(2, 1)*CBL(2, 2) + CBL(3, 1)*CBL(3, 2)) < 1E-10
            flag(1, 2) = 1;
        end
        % 如果C22符合 假设C23并判断
        if flag(1, 2)
            for k = 1:2
                flag(1, [1 3]) = 0;
                CBL(2, 3) = tmpC23(k);
                CBL(1, 3) = (-CBL(2, 1)*CBL(2, 3) - CBL(3, 1)*CBL(3, 3))/CBL(1, 1);
                if (abs(CBL(1, 1)*CBL(1, 3) + CBL(2, 1)*CBL(2, 3) + CBL(3, 1)*CBL(3, 3)) < 1E-10) && (abs(CBL(1, 2)*CBL(1, 3) + CBL(2, 2)*CBL(2, 3) + CBL(3, 2)*CBL(3, 3)) < 1E-10)
                    flag(1, 3) = 1;
                end
                if flag(1, 3) && (det(CBL) - 1) < 1E-10
                    % 如果C23符合 进行综合判断
                    [tmpAzm] = bodyaxisazimuth(CBL);
                    [tmpVertAngle] = bodyaxisvertangle(CBL);
                    % 方位角绝对值判断
                    if (abs(tmpAzm(1) - azm)/pi*180*3600 < 1) || (abs((abs(tmpAzm(1) - azm) - 2*pi))/pi*180*3600 < 1)
                        tmpFlag1 = 1;
                    else
                        tmpFlag1 = 0;
                    end
                    % 方位角关系判断
                    if (abs(tmpAzm(1) - tmpAzm(2) - pi/2)/pi*180 < 10) || (abs(tmpAzm(1) - tmpAzm(2) + 3*pi/2)/pi*180 < 10)
                        tmpFlag2 = 1;
                    else
                        tmpFlag2 = 0;
                    end
                    % 水平角绝对值判断
                    if (abs(tmpVertAngle(1) - vertAngle(1))/pi*180*3600 < 1) && (abs(tmpVertAngle(2) - vertAngle(2))/pi*180*3600 < 1)
                        tmpFlag3 = 1;
                    else
                        tmpFlag3 = 0;
                    end
                    flag(1, 1) = tmpFlag1*tmpFlag2*tmpFlag3;
                end
                if flag(1, 1)
                    break;
                end
            end
        end
        if flag(1, 1)
            break;
        end
    end
    if flag(1, 1)
        break;
    end
end
end